import heapq

class DStar:
    def __init__(self, grid, start, goal):
        self.grid = grid  # 二维网格地图，0表示可通过，1表示障碍
        self.start = start  # 起点
        self.goal = goal  # 终点
        self.rows = len(grid)
        self.cols = len(grid[0])
        self.rhs = {(i, j): float('inf') for i in range(self.rows) for j in range(self.cols)}
        self.g = {(i, j): float('inf') for i in range(self.rows) for j in range(self.cols)}
        self.rhs[self.goal] = 0
        self.open_list = []
        heapq.heappush(self.open_list, (self.calculate_key(self.goal), self.goal))

    def calculate_key(self, node):
        return (min(self.g[node], self.rhs[node]) + self.heuristic(node, self.start),
                min(self.g[node], self.rhs[node]))

    def heuristic(self, node1, node2):
        return abs(node1[0] - node2[0]) + abs(node1[1] - node2[1])

    def neighbors(self, node):
        neighbors = []
        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            nx, ny = node[0] + dx, node[1] + dy
            if 0 <= nx < self.rows and 0 <= ny < self.cols and self.grid[nx][ny] == 0:
                neighbors.append((nx, ny))
        return neighbors

    def update_vertex(self, node):
        if node != self.goal:
            self.rhs[node] = min([self.g[neighbor] + 1 for neighbor in self.neighbors(node)] + [float('inf')])
        if node in [n for _, n in self.open_list]:
            self.open_list.remove((self.calculate_key(node), node))
            heapq.heapify(self.open_list)
        if self.g[node] != self.rhs[node]:
            heapq.heappush(self.open_list, (self.calculate_key(node), node))

    def compute_shortest_path(self):
        while len(self.open_list) > 0 and (self.open_list[0][1] != self.start or self.rhs[self.start] != self.g[self.start]):
            k_old, node = heapq.heappop(self.open_list)
            k_new = self.calculate_key(node)
            if k_old < k_new:
                heapq.heappush(self.open_list, (k_new, node))
            elif self.g[node] > self.rhs[node]:
                self.g[node] = self.rhs[node]
                for neighbor in self.neighbors(node):
                    self.update_vertex(neighbor)
            else:
                self.g[node] = float('inf')
                for neighbor in self.neighbors(node) + [node]:
                    self.update_vertex(neighbor)

    def find_path(self):
        path = []
        current = self.start
        while current != self.goal:
            path.append(current)
            neighbors = self.neighbors(current)
            next_node = min(neighbors, key=lambda n: self.g[n] + 1)
            current = next_node
        path.append(self.goal)
        return path

    def run(self):
        self.compute_shortest_path()
        return self.find_path()


# 示例使用
grid = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0],
    [0, 1, 1, 1, 0],
    [0, 0, 0, 0, 0]
]
start = (0, 0)
goal = (4, 4)

dstar = DStar(grid, start, goal)
path = dstar.run()
print("Path found:", path)
